#include "algorithm_collector.h"
#include <stdio.h>
#include <stdlib.h>

#include <stdint.h>
#include <math.h>
#include <string.h>
#include <stdbool.h>

#ifndef FREE_EXT
#define FREE_EXT(ptr)   \
({  \
    if(ptr){            \
        free(ptr);      \
        ptr = NULL;     \
    }   \
})
#endif // FREE_EXT

//======================================== 二分法查找 ========================================================================
/**
 *@description: 二分法查找,
 *@key: 目标变量的指针,
 *@base: 要查找的数组首地址
 *@num: 要查找的数量
 *@size: 数组元素的大小
 *@cmp: 与目标值比较的函数指针, 目标值 > 当前值: -1, < : 1, = : 0
 *@cmp_last2: 比较最后两个值, 选择左边 : -1, 选择右边 : 1, 都不选择: 0
 *@return:
 *@author: Kalimdorsummer
 *@date: 2021-04-05 22:03:49
*/
void *ks_bsearch(const void *key, const void *base, size_t num, size_t size,
	      int (*cmp)(const void *key, const void *elt), int (*cmp_last2)(const void *key, const void *s, const void *e) )
{
	size_t start = 0, end = num - 1;
	int result;

	while (start < end - 1) {    // start 和 end 紧挨着的时候说明查找完了, 具体是哪个元素, 需要cmp_last2给出结果
		size_t mid = start + (end - start) / 2;

        uint32_t tmp = (uint32_t)base + mid * size;
		result = cmp(key, (void*)tmp);
		if (result < 0){
            end = mid;
        }else if (result > 0){
            start = mid;
        }else{
            tmp = (uint32_t)base + mid * size;
            return (void *)tmp;
        }

	}

	if (cmp_last2 != NULL){
        uint32_t tmp1 = (uint32_t)base + start * size;
        uint32_t tmp2 = (uint32_t)base + end * size;
        result = cmp_last2(key, (void*)tmp1, (void*)tmp2);

        if (result < 0){
            uint32_t tmp = (uint32_t)base + start * size;
            return (void *)tmp;
        }else if (result > 0){
            uint32_t tmp = (uint32_t)base + end * size;
            return (void *)tmp;
        }else{
            return NULL;
        }
	}


	return NULL;
}


#define DEFINE_BSEARCH_CMP(type)    \
int cmp_equ_##type(const void * a, const void * b)    \
{   \
    int ret = *(type*)a - *(type*)b;    \
    return  ret;  \
}   \


DEFINE_BSEARCH_CMP(char)
DEFINE_BSEARCH_CMP(int8_t)
DEFINE_BSEARCH_CMP(uint8_t)
DEFINE_BSEARCH_CMP(int16_t)
DEFINE_BSEARCH_CMP(uint16_t)
DEFINE_BSEARCH_CMP(int32_t)
DEFINE_BSEARCH_CMP(uint32_t)
DEFINE_BSEARCH_CMP(long)
DEFINE_BSEARCH_CMP(int64_t)
DEFINE_BSEARCH_CMP(uint64_t)



//======================================== 二分法查找 end ========================================================================



//======================================== 三次样条插值 ===========================================================================
int spline_init(spl_t v, size_t num_of_points)
{
    v->spline = spline;
    v->get_val = get_val;
    v->set_spline_x = set_spline_x;
    v->set_spline_y = set_spline_y;

    v->n = num_of_points;

    v->x = (double*)malloc(sizeof(double) * num_of_points);
    v->y = (double*)malloc(sizeof(double) * num_of_points);
    v->b = (double*)malloc(sizeof(double) * num_of_points);
    v->c = (double*)malloc(sizeof(double) * num_of_points);
    v->d = (double*)malloc(sizeof(double) * num_of_points);

    bool no_null = v->x && v->x && v->x && v->x && v->x;
    if(!no_null){   // 有分配失败的
        FREE_EXT(v->x);
        FREE_EXT(v->y);
        FREE_EXT(v->b);
        FREE_EXT(v->c);
        FREE_EXT(v->d);
        return -1;
    }

    return 0;
}

int spline_destroy(spl_t v)
{
    FREE_EXT(v->x);
    FREE_EXT(v->y);
    FREE_EXT(v->b);
    FREE_EXT(v->c);
    FREE_EXT(v->d);

    return 0;
}
/**
 *@description: 计算出样条函数系数
 *@param:
 *@return:
 *@author: Kalimdorsummer
 *@date: 2021-04-19 16:12:42
*/

int spline (spl_t v)
{
//    #define n v->n
//    #define x v->x
//    #define y v->y
//    #define b v->b
//    #define c v->c
//    #define d v->d
    int n = v->n;
    double *x = v->x;
    double *y = v->y;
    double *b = v->b;
    double *c = v->c;
    double *d = v->d;

    if (!x || !y || !b || !c || !d){    // 有空指针则报错
        printf("err ptr null\n");
        return -1;
    }

	int nm1, ascend;
	double t;
	nm1 = n - 1;

	int res = 0;

	if ( n < 2 ) {  // 只有一个点无法插值
		res = 1;
        return res;
	}

	ascend = 1;
    // 判断横坐标是单调递增的, 否则不符合插值条件, 退出
	for ( int i = 1; i < n; ++i ) {
        if ( x[i] <= x[i - 1] ) {
            ascend = 0;
        }
	}
	if ( !ascend ) {
		res = 2;
		return res;
	}


	if ( n >= 3 ) {
		d[0] = x[1] - x[0];
		c[1] = ( y[1] - y[0] ) / d[0];

		for (int i = 1; i < nm1; ++i ) {
			d[i] = x[i + 1] - x[i];
			b[i] = 2.0 * ( d[i - 1] + d[i] );
			c[i + 1] = ( y[i + 1] - y[i] ) / d[i];
			c[i] = c[i + 1] - c[i];
		}

		/* ---- Default End conditions */
		b[0] = -d[0];
		b[nm1] = -d[n - 2];
		c[0] = 0.0;
		c[nm1] = 0.0;

		if ( n != 3 ) {
			c[0] = c[2] / ( x[3] - x[1] ) - c[1] / ( x[2] - x[0] );
			c[nm1] = c[n - 2] / ( x[nm1] - x[n - 3] ) - c[n - 3] / ( x[n - 2] - x[n - 4] );
			c[0] = c[0] * d[0] * d[0] / ( x[3] - x[0] );
			c[nm1] = -c[nm1] * d[n - 2] * d[n - 2] / ( x[nm1] - x[n - 4] );
		}

		/* Alternative end conditions -- known slopes */
		if ( v->end1 == 1 ) {
			b[0] = 2.0 * ( x[1] - x[0] );
			c[0] = ( y[1] - y[0] ) / ( x[1] - x[0] ) - v->slope1;
		}

		if ( v->end2 == 1 ) {
			b[nm1] = 2.0 * ( x[nm1] - x[n - 2] );
			c[nm1] = v->slope2 - ( y[nm1] - y[n - 2] ) / ( x[nm1] - x[n - 2] );
		}

		/* Forward elimination */
		for (int i = 1; i < n; ++i ) {
			t = d[i - 1] / b[i - 1];
			b[i] = b[i] - t * d[i - 1];
			c[i] = c[i] - t * c[i - 1];
		}

		/* Back substitution */
		c[nm1] = c[nm1] / b[nm1];

		for ( int ib = 0, i; ib < nm1; ++ib ) {
			i = n - ib - 2;
			c[i] = ( c[i] - d[i] * c[i + 1] ) / b[i];
		}

		b[nm1] = ( y[nm1] - y[n - 2] ) / d[n - 2] + d[n - 2] * ( c[n - 2] + 2.0 * c[nm1] );

		for (int i = 0; i < nm1; ++i ) {
			b[i] = ( y[i + 1] - y[i] ) / d[i] - d[i] * ( c[i + 1] + 2.0 * c[i] );
			d[i] = ( c[i + 1] - c[i] ) / d[i];
			c[i] = 3.0 * c[i];
		}

		c[nm1] = 3.0 * c[nm1];
		d[nm1] = d[n - 2];

	} else {    // 只有两个点时, 就是线段
		b[0] = ( y[1] - y[0] ) / ( x[1] - x[0] );   // 斜率
		c[0] = 0.0;
		d[0] = 0.0;
		b[1] = b[0];
		c[1] = 0.0;
		d[1] = 0.0;
	}

    return res;
}


#define calc_ret_val(horizontal)    \
do{ \
    ret_val = u - x[horizontal]; \
    ret_val = y[horizontal] + ret_val * ( b[horizontal] + ret_val * ( c[horizontal] + ret_val * d[horizontal] ) );  \
}while(0)

// 读出指定自横坐标下的函数值
double get_val ( spl_t v, double u)
{
    int n = v->n;
    double *x = v->x;
    double *y = v->y;
    double *b = v->b;
    double *c = v->c;
    double *d = v->d;

    double ret_val; // 指定的u对应的函数值
    if (u < x[0]){  // 在样本最小值左边
        calc_ret_val(0);
        return ret_val;
    }else if(u > x[n - 1]){
        calc_ret_val(n - 1);
        return ret_val;
    }

    for (int i = 0; i < n; i++){
        // 找到该区间
        if (u >= x[i] && u <= x[i + 1]){
            calc_ret_val(i);
            return ret_val;
        }
    }

    return -1.0f;
}

/**
 *@description: 设置横坐标离散点
 *@param:
 *@return:
 *@author: Kalimdorsummer
 *@date: 2021-05-07 11:15:22
*/
int set_spline_x(spl_t v, double * x_cor, size_t cnt)
{
    if (cnt > v->n){
        return -1;
    }

    memcpy(v->x, x_cor, sizeof(double) * cnt);

    return 0;
}

/**
 *@description: 设置纵坐标离散点
 *@param:
 *@return:
 *@author: Kalimdorsummer
 *@date: 2021-05-07 11:15:56
*/
int set_spline_y(spl_t v, double * y_cor, size_t cnt)
{
    if (cnt > v->n){
        return -1;
    }

    memcpy(v->y, y_cor, sizeof(double) * cnt);

    return 0;
}

//======================================== 三次样条插值 end ===========================================================================


//======================================== polyfit ====================================================================
int polyfit_init(polyfit_t v, size_t poly_data_num, size_t poly_order_n)
{
    v->polyfit = polyfit;
    v->data_num = poly_data_num;
    v->poly_n = poly_order_n;

    v->tempx = (double*)malloc(sizeof(double) * poly_data_num);
    v->tempy = (double*)malloc(sizeof(double) * poly_data_num);
    v->sumxx = (double*)malloc(sizeof(double) * (poly_order_n * 2 + 1) );
    v->sumxy = (double*)malloc(sizeof(double) * (poly_order_n + 1) );
    v->ata   = (double*)malloc(sizeof(double) * (( poly_order_n + 1 ) * ( poly_order_n + 1 )) );

    bool no_null = v->tempx && v->tempy && v->sumxx && v->sumxy && v->ata;

    if(!no_null){
        FREE_EXT(v->tempx);
        FREE_EXT(v->tempy);
        FREE_EXT(v->sumxx);
        FREE_EXT(v->sumxy);
        FREE_EXT(v->ata);
        v->polyfit = NULL;
        v->data_num = 0;
        v->poly_n = 0;
        return -1;
    }

    return 0;
}

int polyfit_destroy(polyfit_t v)
{
    FREE_EXT(v->tempx);
    FREE_EXT(v->tempy);
    FREE_EXT(v->sumxx);
    FREE_EXT(v->sumxy);
    FREE_EXT(v->ata);

    v->polyfit = NULL;
    v->data_num = 0;
    v->poly_n = 0;

    return 0;
}

static void gauss_solve ( int n, double A[], double x[], double b[] );
/*==================polyfit(n,x,y,poly_n,a)===================*/
/*=======拟合y=a0+a1*x+a2*x^2+……+apoly_n*x^poly_n========*/
/*=====n是数据个数 xy是数据值 poly_n是多项式的项数======*/
/*===返回a0,a1,a2,……a[poly_n]，系数比项数多一（常数项）=====*/
void polyfit ( polyfit_t v, double x[], double y[], double a[] )
{
    bool no_null = v->tempx && v->tempy && v->sumxx && v->sumxy && v->ata;
    if(!no_null){
        FREE_EXT(v->tempx);
        FREE_EXT(v->tempy);
        FREE_EXT(v->sumxx);
        FREE_EXT(v->sumxy);
        FREE_EXT(v->ata);
        v->polyfit = NULL;
        v->data_num = 0;
        v->poly_n = 0;
        return;
    }

    double * tempx, *tempy, *sumxx, *sumxy, *ata;

	tempx   = v->tempx;
	tempy   = v->tempy;
	sumxx   = v->sumxx;
	sumxy   = v->sumxy;
	ata     = v->ata;

	size_t n = v->data_num;
	size_t poly_n = v->poly_n;
	for ( int i = 0; i < n; i++ ) {
		tempx[i] = 1;
		tempy[i] = y[i];
	}


	for ( int i = 0; i < 2 * poly_n + 1; i++ ){
        int j;
		for ( sumxx[i] = 0, j = 0; j < n; j++ ) {
			sumxx[i] += tempx[j];
			tempx[j] *= x[j];
		}
	}

	for ( int i = 0; i < poly_n + 1; i++ ){
        int j;
		for ( sumxy[i] = 0, j = 0; j < n; j++ ) {
			sumxy[i] += tempy[j];
			tempy[j] *= x[j];
		}
	}

	for ( int i = 0; i < poly_n + 1; i++ ){
		for ( int j = 0; j < poly_n + 1; j++ ) {
			ata[i * ( poly_n + 1 ) + j] = sumxx[i + j];
		}
	}

	gauss_solve ( poly_n + 1, ata, a, sumxy );

}

static void gauss_solve ( int n, double A[], double x[], double b[] )
{
	int i, j, k, r;
	double max;

	for ( k = 0; k < n - 1; k++ ) {
		max = fabs ( A[k * n + k] ); /*find maxmum*/
		r = k;

		for ( i = k + 1; i < n - 1; i++ )
			if ( max < fabs ( A[i * n + i] ) ) {
				max = fabs ( A[i * n + i] );
				r = i;
			}

		if ( r != k )
			for ( i = 0; i < n; i++ ) { /*change array:A[k]&A[r] */
				max = A[k * n + i];
				A[k * n + i] = A[r * n + i];
				A[r * n + i] = max;
			}

		max = b[k];                  /*change array:b[k]&b[r]     */
		b[k] = b[r];
		b[r] = max;

		for ( i = k + 1; i < n; i++ ) {
			for ( j = k + 1; j < n; j++ ) {
				A[i * n + j] -= A[i * n + k] * A[k * n + j] / A[k * n + k];
			}

			b[i] -= A[i * n + k] * b[k] / A[k * n + k];
		}
	}

	for ( i = n - 1; i >= 0; x[i] /= A[i * n + i], i-- )
		for ( j = i + 1, x[i] = b[i]; j < n; j++ ) {
			x[i] -= A[i * n + j] * x[j];
		}
}

// 获取多项式的值
double get_polyfunc_val(double x, double *a, size_t a_cnt)
{
    double ret = 0.0;

    for(size_t i = 0; i < a_cnt; i++){
        ret += a[i] * pow(x, i);
    }
    return ret;
}
//======================================== polyfit end ====================================================================


//======================================== 删除数组重复元素 ==============================================================
size_t delete_same(void *data_in, size_t n, size_t size, int (*cmp_same)(void *search_val, void *search_same) )
{
    void *search_val = data_in;
    void *tail = (void *)( (uint32_t)data_in + (n - 1) * size);          // &data_in[n - 1];
    void *search_same = tail;
    size_t ret_new_n = n;

    while(search_val < tail){
        while(search_val < search_same){
            if (cmp_same != NULL){
                if (  0 == cmp_same(search_val, search_same)/* *search_val == *search_same */){ // 这是完全一致, 对于浮点数也是
                    memcpy(search_same, tail, size);
                    tail = (void *)((uint32_t)tail - size);
                    ret_new_n--;
                }
            }else{
                if(0 == memcmp(search_val, search_same, size)){
                    memcpy(search_same, tail, size);
                    tail = (void *)((uint32_t)tail - size);
                    ret_new_n--;
                }
            }


            search_same = (void *)( (uint32_t)search_same - size );
        }
        search_same = tail;

        search_val = (void *)( (uint32_t)search_val + size );
    }
    return ret_new_n;

}

//======================================== 删除数组重复元素 end ==============================================================

//======================================== 卡尔曼滤波 ===========================================
static float kalman_filter(kalman_t v, const float data_in);
void kalman_filter_init(kalman_t v, float MeasureNoise_R, float ProcessNiose_Q, float init_state)
{
    v->kalman_filter = kalman_filter;
    v->R = MeasureNoise_R;
    v->Q = ProcessNiose_Q;
    v->x_last = init_state;

}
static float kalman_filter(kalman_t v, const float data_in)
{
    // 状态外推方程和协方差外推方程取决于系统动力学。这里都认为是恒速模型
	v->x_mid = v->x_last;                     //x_last=x(k-1|k-1),x_mid=x(k|k-1), 状态外推方程
	v->p_mid = v->p_last + v->Q;                 //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声, 协方差外推方程 ,系统动力是常数

	/*
	 *  卡尔曼滤波的五个重要公式
	 */
	v->kg = v->p_mid / ( v->p_mid + v->R );                     //kg为kalman filter，R 为噪声, 协方差更新方程
	v->x_now = v->x_mid + v->kg * ( data_in - v->x_mid );     //估计出的最优值
	printf("v->kg: %f\n", v->kg);
	v->p_now = ( 1 - v->kg ) * v->p_mid;                     //最优值对应的covariance
	v->p_last = v->p_now;                                 //更新covariance 值
	v->x_last = v->x_now;                                 //更新系统状态值

	return v->x_now;
}

#ifdef __cplusplus
kalman_s::kalman_s(float MeasureNoise_R, float ProcessNiose_Q, float init_state)
{
    kalman_filter_init(this, MeasureNoise_R, ProcessNiose_Q, init_state);
}

kalman_s::~kalman_s()
{

}

#endif // __cplusplus
//======================================== 卡尔曼滤波 end ===========================================


//======================================== 开方 ==============================================
// 开方精度等级, 0 ~ 2, 0 最最低
static unsigned int sqrt_precision_lvl = 0;
void set_sqrt_precision_lvl(unsigned int lvl)
{
    const unsigned int max_lvl = 3;
    if(lvl > max_lvl){
        sqrt_precision_lvl = max_lvl;
    }else{
        sqrt_precision_lvl = lvl;
    }
}
// 开方
float ks_sqrt ( float x )
{
	float xhalf = 0.5f * x;
	int i = * ( int * ) &x;
	i = 0x5f375a86 - ( i >> 1 );
	x = * ( float * ) &i;

	for(unsigned int i = 0; i < sqrt_precision_lvl; i++){
        x = x * ( 1.5f - xhalf * x * x );
	}

	return 1 / x;
}

// 开方倒数
float ks_sqrt_inv( float number )
{
    long i;
    float x2, y;
    const float threehalfs = 1.5F;

    x2 = number * 0.5F;
    y  = number;
    i  = * ( long * ) &y;                       // evil floating point bit level hacking
    i  = 0x5f375a86 - ( i >> 1 );               // what the fuck?
    y  = * ( float * ) &i;
    for(unsigned int i = 0; i < sqrt_precision_lvl; i++){
        y  = y * ( threehalfs - ( x2 * y * y ) );   // iteration
    }


    return y;
}


//======================================== 开方 end ==============================================
